#!coding:utf-8
import numpy as np
from enum import Enum


class BehaviorState(Enum):
    # 未激活状态
    DEACTIVATED = -1
    # 静止状态
    REST = 0
    # 运动状态
    TROT = 1


class State:
    """
    保存机器人状态
    """

    def __init__(self):
        # 水平速度
        self.horizontal_velocity = np.array([0.0, 0.0])
        # 高度
        self.height = -0.16
        self.roll = 0.0
        self.pitch = 0.0
        self.yaw = 0.0
        # 时钟计数
        self.ticks = 0
        # 脚的坐标
        self.foot_locations = np.zeros((3, 4))
        # 舵机角度
        self.joint_angles = np.zeros((3, 4))
        # 初始状态
        self.behavior_state = BehaviorState.DEACTIVATED


class Command:
    """
    保存机器人指令
    """

    def __init__(self):
        # 水平方向的速度
        self.horizontal_velocity = np.array([0.0, 0.0])
        # 身体高度
        self.height = -0.16
        self.roll = 0.0
        self.pitch = 0.0
        self.yaw = 0.0
        self.trot_event = False
        self.activate_event = False


class Configuration:
    def __init__(self):
        #################### COMMANDS ####################
        # 纵向限速
        self.max_x_velocity = 0.40
        # 横向限速
        self.max_y_velocity = 0.30
        # 转向限速
        self.max_yaw = 2.0
        # 俯仰角限制
        self.max_pitch = 30.0 * np.pi / 180.0
        # 横滚角限制
        self.max_roll = 30.0 * np.pi / 180.0

        #################### MOVEMENT PARAMS ####################

        # z轴限速 [m/s]
        self.z_speed = 0.06
        # 横滚限速 [rad/s]
        self.roll_speed = 0.3


        #################### STANCE ####################
        # 初始站立位置
        self.delta_x = 0.1
        self.delta_y = 0.08
        self.x_shift = 0.0
        # 站立高度
        self.default_z_ref = -0.16

        #################### SWING ######################
        # 摆腿高度
        self.z_clearance = 0.07
        # 腿落下阶段的水平位移和触地阶段水平位移的比例（三角形的腰和底）
        self.alpha = (
            0.5
        )
        self.beta = (
            0.5
        )
        #################### GAIT #######################
        # 时钟周期, 10毫秒为一个周期
        self.dt = 0.01
        # 每个控制周期包含四个相位
        self.num_phases = 4
        # 四条腿的相位，横向看是单条腿的，包含三个落地阶段和一个抬腿阶段
        # 纵向看是整体的，分为四个相位，落地，抬2、3号腿，落地，抬1、4号腿（对角步态）
        self.contact_phases = np.array(
            [[1, 1, 1, 0],
             [1, 0, 1, 1],
             [1, 0, 1, 1],
             [1, 1, 1, 0]]
        )

        # 四只脚都在地面的阶段
        self.overlap_time = (
            0.1
        )
        # 两条腿抬起来的阶段
        self.swing_time = (
            0.15
        )
        # 对应上面四个相位的时间分别是 overlap_time、swing_time、overlap_time、swing_time

        ######################## GEOMETRY ######################
        # 腿和身体连接点的前后距离
        self.LEG_FB = 0.10
        # 腿和身体连接点的左右距离
        self.LEG_LR = 0.04
        # 大腿长度
        self.LEG_L2 = 0.115
        # 小腿长度
        self.LEG_L1 = 0.1235
        # 腿部连接点到腿平面的距离
        self.ABDUCTION_OFFSET = 0.03

        # 以机器人身体中心为坐标原点，四条腿与身体连接点的坐标
        self.LEG_ORIGINS = np.array(
            [
                [self.LEG_FB, self.LEG_FB, -self.LEG_FB, -self.LEG_FB],
                [-self.LEG_LR, self.LEG_LR, -self.LEG_LR, self.LEG_LR],
                [0, 0, 0, 0],
            ]
        )

        # 腿部连接点到腿平面的偏差, 以机器人头部为正面，相对机器人左前上为正向
        self.ABDUCTION_OFFSETS = np.array(
            [
                -self.ABDUCTION_OFFSET,
                self.ABDUCTION_OFFSET,
                -self.ABDUCTION_OFFSET,
                self.ABDUCTION_OFFSET,
            ]
        )

    @property
    # 默认站立姿态的位置
    def default_stance(self):
        return np.array(
            [
                [
                    self.delta_x + self.x_shift,
                    self.delta_x + self.x_shift,
                    -self.delta_x + self.x_shift,
                    -self.delta_x + self.x_shift,
                ],
                [-self.delta_y, self.delta_y, -self.delta_y, self.delta_y],
                [0, 0, 0, 0],
            ]
        )

    ########################### GAIT ####################

    @property
    # 四条腿落地的时钟周期
    def overlap_ticks(self):
        return int(self.overlap_time / self.dt)

    @property
    # 抬腿的时钟周期
    def swing_ticks(self):
        return int(self.swing_time / self.dt)

    @property
    # 落地的时钟周期
    def stance_ticks(self):
        return 2 * self.overlap_ticks + self.swing_ticks

    @property
    # 四个相位分别对应的时钟数
    def phase_ticks(self):
        return np.array(
            [self.overlap_ticks, self.swing_ticks,
                self.overlap_ticks, self.swing_ticks]
        )

    @property
    # 周期长度
    def phase_length(self):
        return 2 * self.overlap_ticks + 2 * self.swing_ticks
